#!/usr/bin/env python3
'''
此代码最为完美，实现接受x,y数值后只动持续1s，不会像之前那样持续运动
author zc
date 2025-04-29
'''
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
import socket
import threading
import time

class SocketControlNode(Node):
    def __init__(self):
        super().__init__('socket_control_node')
        
        # Publisher for cmd_vel
        self.publisher_ = self.create_publisher(Twist, 'cmd_vel', 10)
        self.twist_msg = Twist()
        self.last_publish_time = None
        self.publish_duration = 1.0  # 持续发布2秒
        self.active = False

        # Socket server configuration
        self.host = '0.0.0.0'  # Listen on all interfaces
        self.port = 12345      # Port number
        
        # Start socket server
        self.socket_server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.socket_server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        self.socket_server.bind((self.host, self.port))
        self.socket_server.listen(1)  # Allow 1 client
        self.get_logger().info(f"Socket server started on {self.host}:{self.port}")

        # Start listening in a separate thread
        self.thread = threading.Thread(target=self.listen_for_clients)
        self.thread.daemon = True
        self.thread.start()

        # Timer for checking publish duration
        self.timer = self.create_timer(0.1, self.check_publish_duration)

    def listen_for_clients(self):
        while rclpy.ok():
            try:
                conn, addr = self.socket_server.accept()
                self.get_logger().info(f"Connected by {addr}")
                
                data = conn.recv(1024)
                if not data:
                    conn.close()
                    continue
                
                try:
                    x_str, y_str = data.decode().strip().split(',')
                    x = int(x_str)
                    y = int(y_str)
                    self.get_logger().info(f"Received x: {x}, y: {y}")

                    # Process x (linear velocity)
                    if x < 128:
                        speed = -1.0 * (1.0 - (x / 128.0))  # Backward: 0-128 → -1.0 to 0.0
                    else:
                        speed = (x - 128) / 127.0           # Forward: 128-255 → 0.0 to 1.0

                    # Process y (angular velocity)
                    if y < 128:
                        turn = 1.0 - (y / 128.0)            # Left: 0-128 → 1.0 to 0.0
                    else:
                        turn = -1.0 * ((y - 128) / 127.0)   # Right: 128-255 → 0.0 to -1.0

                    # Update Twist message
                    self.twist_msg.linear.x = speed
                    self.twist_msg.angular.z = turn
                    self.last_publish_time = time.time()
                    self.active = True
                    self.get_logger().info("Started publishing cmd_vel for 2 seconds")

                except ValueError as e:
                    self.get_logger().error(f"Error parsing data: {e}")
                
                # Close current connection
                conn.close()

            except Exception as e:
                self.get_logger().error(f"Socket error: {e}")
                break

    def check_publish_duration(self):
        if self.active and self.last_publish_time is not None:
            if time.time() - self.last_publish_time >= self.publish_duration:
                # Stop publishing after 2 seconds
                self.active = False
                # Optionally publish zero velocity to stop the robot
                self.twist_msg.linear.x = 0.0
                self.twist_msg.angular.z = 0.0
                self.publisher_.publish(self.twist_msg)
                self.get_logger().info("Stopped publishing cmd_vel")
            else:
                # Continue publishing while within the 2-second window
                self.publisher_.publish(self.twist_msg)

    def __del__(self):
        self.socket_server.close()

def main(args=None):
    rclpy.init(args=args)
    node = SocketControlNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
